1. Field of the Invention
This invention relates to a robot control apparatus equipped with a special-purpose control processor for calculating the equivalent inertia of a robot arm load to adjust the loop gain of a servomotor.
2. Description of the Related Art
Horizontal articulated-type robots each of whose arms move in two dimensions have recently come into use for semiconductor device assembly and simple palletizing operations.
FIG. 3 is a simplified external view of an example of a horizontal articulated-type robot. As shown in the Figure, a manipulator 5 is provided on a wrist (hand) 4 at the distal end of a second arm 3. The second arm 3 is provided on the distal end of a first arm 2 so as to be freely pivotable in the horizontal direction. The first arm 2 is fixedly secured to the distal end of a post 1 so as to extend in the horizontal direction. The post 1 is provided on a base 11, which is installed on a floor, and is freely rotated by a drive motor. The first arm 2 accommodates a servomotor for rotating the second arm 3 back and forth, and the second arm 3 accommodates a servomotor for driving the wrist 4.
Numeral 6 denotes a variety of cables such as power cables for supplying power to the servomotors, and signal cables for transmitting signals from various sensors. Numerals 22, 32 denote covers for covering the first arm 2 and second arm 3, respectively. When the respective arm drive systems are subjected to maintenance or inspection, these covers are removed by way of mounting screws.
As shown in FIG. 4, the velocity control system of the the servomotor for driving the robot arms 2, 3 or wrist 4 constitutes a feedback control system which comprises a position and velocity control loop and a current loop and which responds to a input of a predetermined position command.
In the Figure, a position command signal formed by a main CPU, not shown, is altered into a predetermined position control signal by a servo-control circuit fore being sent to a servomotor together with an output signal of a current control loop constituted by a minor loop. A controller is connected to each servo-control circuit. A value indicative of the weight of the hand attached to the wrist 4 and values indicative of the weights of various workpieces gripped in accordance with motion of the manipulator are inputted to each controller from the main CPU. The servomotor controls the manipulator based on the applied control signal. At this time the rotational position of the load is sensed by a rotary encoder and is fed back to the servo-control circuit through a frequency-voltage converter (F/V). K.sub.1 represents integration gain, and K.sub.2 denotes proportional gain. The servomotor is provided for each axis, such as .theta..sub.1 and .theta..sub.2 axes, and the output signal from the rotary encoder for each axis is outputted to the aforementioned controller. The drive torque required for servomotor control is calculated, and a non-linear torque term is compensated by being fed forward to the servo-control circuit.
The torque command u of each servomotor is generally expressed by the following equation: ##EQU1## where J(.theta.) .theta.: equivalent inertia of the load
c(.theta.,.theta.) : centrifugal force, Coriolis force PA1 f(.theta.) : frictional force PA1 g(.theta.) : gravitational force PA1 T(.theta.) : overall non-linear torque
In accordance with conventional servomotor control, the load driving servomotor for each axis, which is connected to and controlled by the main CPU, is provided with a servo-control circuit, and an auxiliary CPU is provided for deciding the non-linear torque, which varies in dependence upon displacement of the manipulator. The auxiliary CPU performs the calculations necessary for feed-forward compensation and forms control signals. Since this auxiliary CPU for a calculating drive torque is required seperately of arithmetic means provided for every servo-control circuit for the purpose of calculating the position command, the cost of the control apparatus is high and the control of data between the CPUs is complicated.
Each servo CPU calculates the equivalent inertia of the load and the non-linear torque required for controlling the servomotor in accordance with equation (a). At such time it is required that the CPU accept such information as the rotational angles about the other axes. In consequence, the current loop processing performed by each servo CPU is impeded and servomotor current control can no longer be carried out accurately.
Furthermore, the hand and workpeice weight values inputted to the controller in order for drive torque to be calculated for the servo-control circuit of each servomotor are preset in the control program of the main CPU. Therefore, when processing is executed with regard to a workpiece having a weight other than that stipulated, the operator must modify the program. This requires a great deal of labor.